/*
 * MapTansform.cpp
 *
 *  Created on: Oct 24, 2010
 *      Author: sanjeev
 */

#include "MapTransform.h"

MapTransform::MapTransform(){
	get_map_odom_transform();
}

MapTransform::~MapTransform() {
}

void MapTransform::updateMapOdomTransform(){
	get_map_odom_transform();
}

void MapTransform::point_in_map(double &x,double &y){
	tf::Pose pose, MapPose;
	geometry_msgs::Pose geoMapPose;

	pose = btTransform(tf::createQuaternionFromYaw(0), btVector3(x,y,0));
	MapPose = rtransform * pose;

	tf::poseTFToMsg(MapPose,geoMapPose);
	x = geoMapPose.position.x;
	y = geoMapPose.position.y;
}

void MapTransform::get_map_odom_transform( ){
	for(int i=0;i<3;i++){
		try{
			ros::Time now = ros::Time::now();
			listener.waitForTransform("/map", "/odom", now, ros::Duration(1.0));
			listener.lookupTransform("/map", "/odom", now, transform);
			listener.lookupTransform("/map", "/odom", now, rtransform);
			printf("[[[[[ %d %lf %lf\n",i,transform.getOrigin().x(),transform.getOrigin().y());
			break;
		}
		catch(tf::TransformException& ex)
		{
			std::cout << "Exception thrown:" << ex.what()<< std::endl;
		}
	}
}

void MapTransform::get_footprint_in_odom(double &x,double &y,double &t){
	std::string source_frameid = "odom";
	std::string target_frameid = "base_footprint";
	try{
		listener.waitForTransform(source_frameid, target_frameid, ros::Time(), ros::Duration(10.0));
		listener.lookupTransform(source_frameid, target_frameid, ros::Time(), transform);
	}
	catch(tf::TransformException& ex)
	{
		ROS_ERROR("Footprint missing");
		std::cout << "Failure at "<< ros::Time::now() << std::endl;
		std::cout << "Exception thrown:" << ex.what()<< std::endl;
	}
	x = transform.getOrigin().x();
	y = transform.getOrigin().y();
	t = get_yaw(transform);
}
